// #include "a_star.h"
#include <mc_path_planning/a_star/a_star.h>

Astar::Astar(){
    LOG(INFO) << "AstarPlanner initializtion......";

    for (int x = -1; x <= 1; x++)
        for (int y = -1; y <= 1; y++){
            if (x == 0 && y == 0) continue;
            std::shared_ptr<GP> new_gp = std::make_shared<GP>(x, y, hypot(x,y), 0, 0, nullptr);
            __neighbor_points.emplace_back(new_gp);
        }
}

Astar::~Astar(){

}



// 全局路径规划 虚函数接口
bool Astar::PathGenerate(Vec2i start, Vec2i goal, vector<Vec2i>& points){

    points.clear();
    std::priority_queue<std::shared_ptr<GP>, std::vector<std::shared_ptr<GP>>, GP::compare_cost> open_list;
    std::unordered_map<int, std::shared_ptr<GP>> closed_list;
    vector<std::shared_ptr<GP>> points_gp, expend_points_gp;

    std::shared_ptr<GP> gp_start = std::make_shared<GP>(start.x, start.y, 0.0, 0.0, ToIndex(start), nullptr);
    std::shared_ptr<GP> gp_goal = std::make_shared<GP>(goal.x, goal.y, 0.0, 0.0, ToIndex(goal), nullptr);
    gp_start->setH(hypot(gp_goal->x - gp_start->x, gp_goal->y - gp_start->y) * 1.5);
    open_list.push(gp_start);

    while(!ros::isShuttingDown() && !open_list.empty()){
        // 获取优先级最高节点
        std::shared_ptr<GP> current = open_list.top();
        open_list.pop();

        if(closed_list.find(current->idx) != closed_list.end())
            continue; 
        closed_list.insert(std::make_pair(current->idx, current));

        expend_points_gp.push_back(current);

        // 找到终点，获取路径
        if(current->idx == gp_goal->idx){
            cout << "find goal. open_list size: " << open_list.size() << endl;
            while(current->getParent() != nullptr){
                points_gp.emplace_back(current);
                current = current->getParent();
            }
            std::reverse(points_gp.begin(), points_gp.end());

            for(auto gp : points_gp){
                points.emplace_back(Vec2i(gp->x, gp->y));
            }
            return true;
        }

        // 遍历邻近节点
        for(auto gp : __neighbor_points){
            std::shared_ptr<GP> new_gp = std::make_shared<GP>();
            new_gp->setX(current->x + gp->x);
            new_gp->setY(current->y + gp->y);
            new_gp->setG(current->g + gp->g);
            new_gp->setH(current->h + gp->h);
            new_gp->setIdx(ToIndex(Vec2i(new_gp->x, new_gp->y)));   

            if(closed_list.find(new_gp->idx) != closed_list.end()){
                continue;
            }    

            if(!IsValid(new_gp->idx) || !ValueIsValid(Vec2i(new_gp->x, new_gp->y))){
                continue;
            }

            new_gp->setParent(current);
            new_gp->setH(hypot(gp_goal->x - new_gp->x, gp_goal->y - new_gp->y) * 1.5);           
            open_list.push(new_gp);
        }             
    }
    return false;
}
